﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Drawing;

namespace MonoPhysics
{
    /// <summary>
    /// Moteur physique.
    /// </summary>
    public static class MathGeometryHelper
    {
        // positif: a gauche // negatif : a droite
        public static float DistPtDrt(PointD ptd1, PointD ptd2, PointD pt)
        {
            float dx = ptd2.X - ptd1.X;
            float dy = ptd2.Y - ptd1.Y;
            if (dx == 0 && dy == 0)
                return 0;
            return (float)((dy * (pt.X - ptd1.X) - dx * (pt.Y - ptd1.Y)) / Math.Sqrt(dx * dx + dy * dy));
        }

        public static float SqDistPtSeg(PointD a, PointD b, PointD c)
        {
            Vector ab = new Vector(a, b);
            Vector ac = new Vector(a, c);
            Vector bc = new Vector(b, c);
            float e = ac.dot(ab);
            if (e <= 0) return ac.dot(ac);
            float f = ab.dot(ab);
            if (e >= f) return bc.dot(bc);
            return ac.dot(ac) - e * e / f;
        }

        public static bool isRight(PointD ptd1, PointD ptd2, PointD pt)
        {
            float dx = ptd2.X - ptd1.X;
            float dy = ptd2.Y - ptd1.Y;
            if (dx == 0 && dy == 0)
                return false;
            return (dy * (pt.X - ptd1.X) - dx * (pt.Y - ptd1.Y)) < 0;
        }

        #region Aire
        public static float CalcConvexSurface(Polygon poly, out bool clockWinse)
        {
            CircularLinkedListNode<PointD, RigidBody> pts = poly.Points;
            CircularLinkedListNode<PointD, RigidBody> parc = poly.Points;
            float a = 0;
            PointD pt0 = new PointD(0, 0);
            do
            {
                a += (parc.value.X * parc.nodes[1].fils.value.Y - parc.value.Y * parc.nodes[1].fils.value.X);
                parc = parc.nodes[1].fils;
            }
            while (parc != pts);
            clockWinse = (a < 0);
            return Math.Abs(a / 2);
        }

        public static float CalcConvexSurface(List<PointD> poly, out bool clockWinse)
        {
            int l = poly.Count;
            float a = 0;
            int j = l - 1;
            for (int i = 0; i < l; i++)
            {
                a += poly[j].X * poly[i].Y - poly[j].Y * poly[i].X;
                j = i;
            }
            clockWinse = (a < 0);
            return Math.Abs(a / 2);
        }
        #endregion

        #region Moment d'inertie
        public static float CalcInertia(List<PointD> poly, float m)
        {
            float num = 0;
            float denum = 0;
            int l = poly.Count;
            int j = l - 1;
            for (int i = 0; i < l; i++)
            {
                Vector pn = new Vector(poly[j]);
                Vector pn1 = new Vector(poly[i]);
                float f = pn1.cross(pn).Magnitude;
                denum += f;
                num += f * (pn1.dot(pn1) + pn1.dot(pn) + pn.dot(pn));
                j = i;
            }
            return (denum == 0) ? 0 : m * num / (6 * denum);
        }

        public static float CalcInertia(Polygon poly, float m)
        {
            float num = 0;
            float denum = 0;
            CircularLinkedListNode<PointD, RigidBody> pts = poly.Points;
            CircularLinkedListNode<PointD, RigidBody> parc = poly.Points;
            do
            {
                Vector pn = new Vector(parc.value);
                Vector pn1 = new Vector(parc.nodes[1].fils.value);
                float f = pn1.cross(pn).Magnitude;
                denum += f;
                num += f * (pn1.dot(pn1) + pn1.dot(pn) + pn.dot(pn));
                parc = parc.nodes[1].fils;
            }
            while (parc != pts);
            return (denum == 0) ? 0 : m * num / (6 * denum);
        }
        #endregion

        #region Root finding
        public delegate float UnaryFunction(float x);
        public static float? BrentFindRoot(UnaryFunction f, float a, float b, float epsilon)
        {
            float fa = f(a);
            float fb = f(b);
            float fc;
            if (fa * fb >= 0)
                return null;
            if (Math.Abs(fa) < Math.Abs(fb))
                swap<float>(ref a, ref b);
            float c = a;
            float d = 0; // sera initialise dans la premiere iteration
            bool mflag = true;
            float s;
            while (fb != 0 && Math.Abs(b - a) > epsilon)
            {
                fa = f(a);
                fb = f(b);
                fc = f(c);

                if ((fa != fc) && (fb != fc))
                    s = ((a * fb * fc) / ((fa - fb) * (fa - fc))) +
                        ((b * fa * fc) / ((fb - fa) * (fb - fc))) +
                        ((c * fa * fb) / ((fc - fa) * (fc - fb)));
                else
                    s = b - fb * ((b - a) / (fb - fa));

                if ((!(s >= (3 * a + b) / 4 && s <= b) &&
                    !(s <= (3 * a + b) / 4 && s >= b)) ||
                    (mflag && Math.Abs(s - b) >= Math.Abs(b - c) / 2) ||
                    (!mflag && Math.Abs(s - b) >= Math.Abs(c - d) / 2))
                {
                    s = (a + b) / 2;
                    mflag = true;
                }
                else
                    mflag = false;
                float fs = f(s);
                d = c;
                c = b;
                if (fa * fs < 0)
                    b = s;
                else
                    a = s;
                fa = f(a);
                fb = f(b);
                if (Math.Abs(fa) < Math.Abs(fb))
                    swap<float>(ref a, ref b);
            }
            return b;
        }
        #endregion

        public static void swap<T>(ref T a, ref T b)
        {
            T c = a;
            a = b;
            b = c;
        }

    }


}
